#include <Servo.h>


const short   SERIAL_RATE       = 9600;

// smithre5: Add an entry to servo_id if you're going to extend any of these
// as SERVO_COUNT depends on the number of entries in the enum.

// smithre5: Neither the min or max us values are correct yet, though they
// seem to minimize the amount of grinding in the servos.

const byte    SERVO_PINS[]      = { 3, 4, };
const short   SERVO_MIN_US[]    = { 440, 440 };
const short   SERVO_MAX_US[]    = { 1800, 1800 };

char serialReadBuffer[4];

enum servo_id
{
  S0L,
  S0R,
  S1L,
  S1R,
  
  SERVO_COUNT
};

Servo servos[SERVO_COUNT];


void setup()
{
  for (int i = SERVO_COUNT; --i >= 0;) {
    servos[i].attach(SERVO_PINS[i], SERVO_MIN_US[i], SERVO_MAX_US[i]);
    servos[i].write(0);
  }
  
  Serial.begin(SERIAL_RATE);
}


void loop()
{
  if (Serial.readBytesUntil('\n', serialReadBuffer, 4) > 0) {
    serialReadBuffer[3] = 0;
    byte angle = atoi(serialReadBuffer);
    servos[S0L].write(angle);
    servos[S0R].write(angle);
  }
}
